<!DOCTYPE html>
<html xmlns="http://www.w3.org/1999/xhtml" lang xml:lang>
<head>
  <meta charset="utf-8" />
  <meta name="generator" content="pandoc" />
  <meta name="viewport" content="width=device-width, initial-scale=1.0, user-scalable=yes" />
  <title>object_detector_3d ROSパッケージ</title>
  <style type="text/css">
      code{white-space: pre-wrap;}
      span.smallcaps{font-variant: small-caps;}
      span.underline{text-decoration: underline;}
      div.column{display: inline-block; vertical-align: top; width: 50%;}
  </style>
  <style type="text/css">
a.sourceLine { display: inline-block; line-height: 1.25; }
a.sourceLine { pointer-events: none; color: inherit; text-decoration: inherit; }
a.sourceLine:empty { height: 1.2em; }
.sourceCode { overflow: visible; }
code.sourceCode { white-space: pre; position: relative; }
div.sourceCode { margin: 1em 0; }
pre.sourceCode { margin: 0; }
@media screen {
div.sourceCode { overflow: auto; }
}
@media print {
code.sourceCode { white-space: pre-wrap; }
a.sourceLine { text-indent: -1em; padding-left: 1em; }
}
pre.numberSource a.sourceLine
  { position: relative; left: -4em; }
pre.numberSource a.sourceLine::before
  { content: attr(data-line-number);
    position: relative; left: -1em; text-align: right; vertical-align: baseline;
    border: none; pointer-events: all; display: inline-block;
    -webkit-touch-callout: none; -webkit-user-select: none;
    -khtml-user-select: none; -moz-user-select: none;
    -ms-user-select: none; user-select: none;
    padding: 0 4px; width: 4em;
    color: #aaaaaa;
  }
pre.numberSource { margin-left: 3em; border-left: 1px solid #aaaaaa;  padding-left: 4px; }
div.sourceCode
  {  }
@media screen {
a.sourceLine::before { text-decoration: underline; }
}
code span.al { color: #ff0000; font-weight: bold; } /* Alert */
code span.an { color: #60a0b0; font-weight: bold; font-style: italic; } /* Annotation */
code span.at { color: #7d9029; } /* Attribute */
code span.bn { color: #40a070; } /* BaseN */
code span.bu { } /* BuiltIn */
code span.cf { color: #007020; font-weight: bold; } /* ControlFlow */
code span.ch { color: #4070a0; } /* Char */
code span.cn { color: #880000; } /* Constant */
code span.co { color: #60a0b0; font-style: italic; } /* Comment */
code span.cv { color: #60a0b0; font-weight: bold; font-style: italic; } /* CommentVar */
code span.do { color: #ba2121; font-style: italic; } /* Documentation */
code span.dt { color: #902000; } /* DataType */
code span.dv { color: #40a070; } /* DecVal */
code span.er { color: #ff0000; font-weight: bold; } /* Error */
code span.ex { } /* Extension */
code span.fl { color: #40a070; } /* Float */
code span.fu { color: #06287e; } /* Function */
code span.im { } /* Import */
code span.in { color: #60a0b0; font-weight: bold; font-style: italic; } /* Information */
code span.kw { color: #007020; font-weight: bold; } /* Keyword */
code span.op { color: #666666; } /* Operator */
code span.ot { color: #007020; } /* Other */
code span.pp { color: #bc7a00; } /* Preprocessor */
code span.sc { color: #4070a0; } /* SpecialChar */
code span.ss { color: #bb6688; } /* SpecialString */
code span.st { color: #4070a0; } /* String */
code span.va { color: #19177c; } /* Variable */
code span.vs { color: #4070a0; } /* VerbatimString */
code span.wa { color: #60a0b0; font-weight: bold; font-style: italic; } /* Warning */
  </style>
  <style type="text/css">@font-face {
font-family: octicons-link;
src: url(data:font/woff;charset=utf-8;base64,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) format('woff');
}
body {
-webkit-text-size-adjust: 100%;
text-size-adjust: 100%;
color: #333;
font-family: "Helvetica Neue", Helvetica, "Segoe UI", Arial, freesans, sans-serif, "Apple Color Emoji", "Segoe UI Emoji", "Segoe UI Symbol";
font-size: 16px;
line-height: 1.6;
word-wrap: break-word;
width: 728px;
max-width: 99%;
box-sizing: border-box;
padding: 30px 30px 8rem 30px;
margin-left: auto;
margin-right: auto;
}
body a {
background-color: transparent;
}
body a:active,
body a:hover {
outline: 0;
}
body strong {
font-weight: bold;
}
body h1 {
font-size: 2em;
margin: 0.67em 0;
}
body img {
border: 0;
}
body hr {
box-sizing: content-box;
height: 0;
}
body pre {
overflow: auto;
}
body code,
body kbd,
body pre {
font-family: monospace, monospace;
font-size: 1em;
}
body input {
color: inherit;
font: inherit;
margin: 0;
}
body html input[disabled] {
cursor: default;
}
body input {
line-height: normal;
}
body input[type="checkbox"] {
box-sizing: border-box;
padding: 0;
}
body table {
border-collapse: collapse;
border-spacing: 0;
}
body td,
body th {
padding: 0;
}
body * {
box-sizing: border-box;
}
body input {
font: 13px / 1.4 Helvetica, arial, nimbussansl, liberationsans, freesans, clean, sans-serif, "Apple Color Emoji", "Segoe UI Emoji", "Segoe UI Symbol";
}
body a {
color: #4078c0;
text-decoration: none;
}
body a:hover,
body a:active {
text-decoration: underline;
}
body hr {
height: 0;
margin: 15px 0;
overflow: hidden;
background: transparent;
border: 0;
border-bottom: 1px solid #ddd;
}
body hr:before {
display: table;
content: "";
}
body hr:after {
display: table;
clear: both;
content: "";
}
body h1,
body h2,
body h3,
body h4,
body h5,
body h6 {
margin-top: 15px;
margin-bottom: 15px;
line-height: 1.1;
}
body h1 {
font-size: 30px;
}
body h2 {
font-size: 21px;
}
body h3 {
font-size: 16px;
}
body h4 {
font-size: 14px;
}
body h5 {
font-size: 12px;
}
body h6 {
font-size: 11px;
}
body blockquote {
margin: 0;
}
body ul,
body ol {
padding: 0;
margin-top: 0;
margin-bottom: 0;
}
body ol ol,
body ul ol {
list-style-type: lower-roman;
}
body ul ul ol,
body ul ol ol,
body ol ul ol,
body ol ol ol {
list-style-type: lower-alpha;
}
body dd {
margin-left: 0;
}
body code {
font-family: Consolas, "Liberation Mono", Menlo, Courier, monospace;
font-size: 12px;
}
body pre {
margin-top: 0;
margin-bottom: 0;
font: 12px Consolas, "Liberation Mono", Menlo, Courier, monospace;
}
body .select::-ms-expand {
opacity: 0;
}
body .octicon {
font: normal normal normal 16px/1 octicons-link;
display: inline-block;
text-decoration: none;
text-rendering: auto;
-webkit-font-smoothing: antialiased;
-moz-osx-font-smoothing: grayscale;
-webkit-user-select: none;
-moz-user-select: none;
-ms-user-select: none;
user-select: none;
}
body .octicon-link:before {
content: '\f05c';
}
body:before {
display: table;
content: "";
}
body:after {
display: table;
clear: both;
content: "";
}
body>*:first-child {
margin-top: 0 !important;
}
body>*:last-child {
margin-bottom: 0 !important;
}
body a:not([href]) {
color: inherit;
text-decoration: none;
}
body .anchor {
display: inline-block;
padding-right: 2px;
margin-left: -18px;
}
body .anchor:focus {
outline: none;
}
body h1,
body h2,
body h3,
body h4,
body h5,
body h6 {
margin-top: 1em;
margin-bottom: 16px;
font-weight: bold;
line-height: 1.4;
}
body h1 .octicon-link,
body h2 .octicon-link,
body h3 .octicon-link,
body h4 .octicon-link,
body h5 .octicon-link,
body h6 .octicon-link {
color: #000;
vertical-align: middle;
visibility: hidden;
}
body h1:hover .anchor,
body h2:hover .anchor,
body h3:hover .anchor,
body h4:hover .anchor,
body h5:hover .anchor,
body h6:hover .anchor {
text-decoration: none;
}
body h1:hover .anchor .octicon-link,
body h2:hover .anchor .octicon-link,
body h3:hover .anchor .octicon-link,
body h4:hover .anchor .octicon-link,
body h5:hover .anchor .octicon-link,
body h6:hover .anchor .octicon-link {
visibility: visible;
}
body h1 {
padding-bottom: 0.3em;
font-size: 1.75em;
line-height: 1.2;
}
body h1 .anchor {
line-height: 1;
}
body h2 {
padding-bottom: 0.3em;
font-size: 1.5em;
line-height: 1.225;
}
body h2 .anchor {
line-height: 1;
}
body h3 {
font-size: 1.25em;
line-height: 1.43;
}
body h3 .anchor {
line-height: 1.2;
}
body h4 {
font-size: 1em;
}
body h4 .anchor {
line-height: 1.2;
}
body h5 {
font-size: 1em;
}
body h5 .anchor {
line-height: 1.1;
}
body h6 {
font-size: 1em;
color: #777;
}
body h6 .anchor {
line-height: 1.1;
}
body p,
body blockquote,
body ul,
body ol,
body dl,
body table,
body pre {
margin-top: 0;
margin-bottom: 16px;
}
body hr {
height: 4px;
padding: 0;
margin: 16px 0;
background-color: #e7e7e7;
border: 0 none;
}
body ul,
body ol {
padding-left: 2em;
}
body ul ul,
body ul ol,
body ol ol,
body ol ul {
margin-top: 0;
margin-bottom: 0;
}
body li>p {
margin-top: 16px;
}
body dl {
padding: 0;
}
body dl dt {
padding: 0;
margin-top: 16px;
font-size: 1em;
font-style: italic;
font-weight: bold;
}
body dl dd {
padding: 0 16px;
margin-bottom: 16px;
}
body blockquote {
padding: 0 15px;
color: #777;
border-left: 4px solid #ddd;
}
body blockquote>:first-child {
margin-top: 0;
}
body blockquote>:last-child {
margin-bottom: 0;
}
body table {
display: block;
width: 100%;
overflow: auto;
word-break: normal;
word-break: keep-all;
}
body table th {
font-weight: bold;
}
body table th,
body table td {
padding: 6px 13px;
border: 1px solid #ddd;
}
body table tr {
background-color: #fff;
border-top: 1px solid #ccc;
}
body table tr:nth-child(2n) {
background-color: #f8f8f8;
}
body img {
max-width: 100%;
box-sizing: content-box;
background-color: #fff;
}
body code {
padding: 0;
padding-top: 0;
padding-bottom: 0;
margin: 0;
font-size: 85%;
background-color: rgba(0,0,0,0.04);
border-radius: 3px;
}
body code:before,
body code:after {
letter-spacing: -0.2em;
content: "\00a0";
}
body pre>code {
padding: 0;
margin: 0;
font-size: 100%;
word-break: normal;
white-space: pre;
background: transparent;
border: 0;
}
body .highlight {
margin-bottom: 16px;
}
body .highlight pre,
body pre {
padding: 16px;
overflow: auto;
font-size: 85%;
line-height: 1.45;
background-color: #f7f7f7;
border-radius: 3px;
}
body .highlight pre {
margin-bottom: 0;
word-break: normal;
}
body pre {
word-wrap: normal;
}
body pre code {
display: inline;
max-width: initial;
padding: 0;
margin: 0;
overflow: initial;
line-height: inherit;
word-wrap: normal;
background-color: transparent;
border: 0;
}
body pre code:before,
body pre code:after {
content: normal;
}
body kbd {
display: inline-block;
padding: 3px 5px;
font-size: 11px;
line-height: 10px;
color: #555;
vertical-align: middle;
background-color: #fcfcfc;
border: solid 1px #ccc;
border-bottom-color: #bbb;
border-radius: 3px;
box-shadow: inset 0 -1px 0 #bbb;
}
body .pl-c {
color: #969896;
}
body .pl-c1,
body .pl-s .pl-v {
color: #0086b3;
}
body .pl-e,
body .pl-en {
color: #795da3;
}
body .pl-s .pl-s1,
body .pl-smi {
color: #333;
}
body .pl-ent {
color: #63a35c;
}
body .pl-k {
color: #a71d5d;
}
body .pl-pds,
body .pl-s,
body .pl-s .pl-pse .pl-s1,
body .pl-sr,
body .pl-sr .pl-cce,
body .pl-sr .pl-sra,
body .pl-sr .pl-sre {
color: #183691;
}
body .pl-v {
color: #ed6a43;
}
body .pl-id {
color: #b52a1d;
}
body .pl-ii {
background-color: #b52a1d;
color: #f8f8f8;
}
body .pl-sr .pl-cce {
color: #63a35c;
font-weight: bold;
}
body .pl-ml {
color: #693a17;
}
body .pl-mh,
body .pl-mh .pl-en,
body .pl-ms {
color: #1d3e81;
font-weight: bold;
}
body .pl-mq {
color: #008080;
}
body .pl-mi {
color: #333;
font-style: italic;
}
body .pl-mb {
color: #333;
font-weight: bold;
}
body .pl-md {
background-color: #ffecec;
color: #bd2c00;
}
body .pl-mi1 {
background-color: #eaffea;
color: #55a532;
}
body .pl-mdr {
color: #795da3;
font-weight: bold;
}
body .pl-mo {
color: #1d3e81;
}
body kbd {
display: inline-block;
padding: 3px 5px;
font: 11px Consolas, "Liberation Mono", Menlo, Courier, monospace;
line-height: 10px;
color: #555;
vertical-align: middle;
background-color: #fcfcfc;
border: solid 1px #ccc;
border-bottom-color: #bbb;
border-radius: 3px;
box-shadow: inset 0 -1px 0 #bbb;
}
body .task-list-item {
list-style-type: none;
}
body .task-list-item+.task-list-item {
margin-top: 3px;
}
body .task-list-item input {
margin: 0 0.35em 0.25em -1.6em;
vertical-align: middle;
}
body :checked+.radio-label {
z-index: 1;
position: relative;
border-color: #4078c0;
}
</style>
  <!--[if lt IE 9]>
    <script src="//cdnjs.cloudflare.com/ajax/libs/html5shiv/3.7.3/html5shiv-printshiv.min.js"></script>
  <![endif]-->
</head>
<body>
<nav id="TOC">
<ul>
<li><a href="#object_detector_3d-rosパッケージ">object_detector_3d ROSパッケージ</a></li>
<li><a href="#概要">概要</a><ul>
<li><a href="#動作環境">動作環境</a></li>
</ul></li>
<li><a href="#使用方法">使用方法</a><ul>
<li><a href="#インストール">インストール</a></li>
</ul></li>
<li><a href="#rosノード">ROSノード</a><ul>
<li><a href="#購読トピック">購読トピック</a></li>
<li><a href="#出版トピック">出版トピック</a></li>
</ul></li>
<li><a href="#実装の詳細">実装の詳細</a><ul>
<li><a href="#入出力データ">入出力データ</a></li>
<li><a href="#アルゴリズムの概要">アルゴリズムの概要</a></li>
<li><a href="#各アルゴリズムの説明">各アルゴリズムの説明</a><ul>
<li><a href="#d物体検出">(1) 2D物体検出</a></li>
<li><a href="#部分点群の抽出">(2, 3)部分点群の抽出</a></li>
<li><a href="#部分点群の中心点の算出">(4) 部分点群の中心点の算出</a></li>
<li><a href="#d検出と3d中心点座標の統合">(5) 2D検出と3D中心点座標の統合</a></li>
</ul></li>
</ul></li>
<li><a href="#ソフトウェアの保守拡張について">ソフトウェアの保守・拡張について</a><ul>
<li><a href="#独自の2d物体検出器を利用する場合">独自の2D物体検出器を利用する場合</a></li>
<li><a href="#realsense以外のセンサーを利用する場合">RealSense以外のセンサーを利用する場合</a></li>
</ul></li>
</ul>
</nav>
<h1 id="object_detector_3d-rosパッケージ">object_detector_3d ROSパッケージ</h1>
<p>[TOC]</p>
<h1 id="概要">概要</h1>
<p>RGDBカメラにより計測する画像と点群を用いて、環境中の複数の物体を検出し、各物体の種類と3次元座標を取得することができるROSパッケージを作成した。</p>
<h2 id="動作環境">動作環境</h2>
<p>以下の環境で動作確認している。</p>
<ul>
<li>Ubuntu 16.04.6</li>
<li>ROS kinetic</li>
<li>Python 2.7.16</li>
<li>Intel RealSense D435</li>
</ul>
<h1 id="使用方法">使用方法</h1>
<p><code>object_detector_3d</code> ディレクトリが、インストール可能なROSパッケージとなっている。後述の方法によりインストールした後、 D435を接続した状態で次のコマンドを実行することで検出器が作動する:</p>
<div class="sourceCode" id="cb1"><pre class="sourceCode bash"><code class="sourceCode bash"><a class="sourceLine" id="cb1-1" data-line-number="1">$ <span class="ex">roslaunch</span> object_detector_3d run.launch</a></code></pre></div>
<p>そうすると、物体の3次元検出結果が <code>/object_detection_3d</code> という名前のトピックから <code>object_detector_3d/msg</code> に定義されている <code>Detection3DResult</code> 型の メッセージとして発行される。 <code>Detection3DResult</code> 型は複数個の <code>Detection3D</code> 型メッセージをまとめたものであり、個別の検出は <code>Detection3D</code> 型により表現されている。</p>
<p>また、検出結果を視覚的に確認できるように、 <code>/object_detection_3d/result_image</code> トピックから <code>sensor_msgs.Image</code> 型のメッセージを発行することができる。</p>
<p>実行中の様子は次の図のようになる。<img src="" alt="実行中の様子" /></p>
<ul>
<li>図の左側には <code>rostopic echo /object_detection_3d</code> による出力が表示されている。</li>
<li>図の右上には <code>rviz</code> 経由で <code>/object_detection_3d/result_image</code> が表示されている。</li>
</ul>
<p>右上を見るとこの場面には視点の手前側からキーボード、マグカップ、ボトル、2台のモニターがあり、それぞれが検出されていることがわかる。各検出ごとにキャプションがついており、次の情報が表示されている:</p>
<ul>
<li>物体の名前。</li>
<li>検出のスコア。区間 <code>[0, 1]</code> に含まれる数値であって、1に近いほど確信度が高い。</li>
<li>物体の中心点（後述）の3次元座標。座標系は原点がカメラの中心であり、x, y, z軸の方向はそれぞれ右、下、奥向きで、単位はメートルである。</li>
</ul>
<p>5つの検出結果について、特に奥行方向の距離であるzの値に注目すると、物体の位置が手前から奥に遠のくのにつれて値が大きくなっていることが確認できる。</p>
<h2 id="インストール">インストール</h2>
<p>あらかじめROSの環境構築は済んでいるとする。またPythonは2.7であり、numpyやmatplotlibなどの主要なパッケージはインストール済みであるとする。</p>
<p>そのうえで、以下のコマンドにより依存ライブラリをインストールする。</p>
<div class="sourceCode" id="cb2"><pre class="sourceCode bash"><code class="sourceCode bash"><a class="sourceLine" id="cb2-1" data-line-number="1">$ <span class="ex">pip</span> install pyrealsense2 chainer chainercv</a>
<a class="sourceLine" id="cb2-2" data-line-number="2">$ <span class="bu">cd</span> ~/catkin_ws/src</a>
<a class="sourceLine" id="cb2-3" data-line-number="3">$ <span class="fu">git</span> clone https://github.com/eric-wieser/ros_numpy.git</a></code></pre></div>
<p>さらに、 <code>object_detector_3d</code> ディレクトリを <code>~/catkin_ws/src</code> に配置し、</p>
<div class="sourceCode" id="cb3"><pre class="sourceCode bash"><code class="sourceCode bash"><a class="sourceLine" id="cb3-1" data-line-number="1">$ <span class="bu">cd</span> ~/catkin_ws/</a>
<a class="sourceLine" id="cb3-2" data-line-number="2">$ <span class="ex">catkin_make</span></a></code></pre></div>
<p>によってインストールする。</p>
<p>なお、Pythonの正確な環境情報は <code>py2_object_detector_3d.yml</code> に記載されている。Anacondaを利用している場合は、</p>
<div class="sourceCode" id="cb4"><pre class="sourceCode bash"><code class="sourceCode bash"><a class="sourceLine" id="cb4-1" data-line-number="1">$ <span class="ex">conda</span> env create -f py2_object_detector_3d.yml</a></code></pre></div>
<p>により環境をインポートしたうえで、</p>
<div class="sourceCode" id="cb5"><pre class="sourceCode bash"><code class="sourceCode bash"><a class="sourceLine" id="cb5-1" data-line-number="1">$ <span class="ex">conda</span> activate py2_object_detector_3d</a></code></pre></div>
<p>により環境を再現することができる。</p>
<h1 id="rosノード">ROSノード</h1>
<p><code>/object_detector_3d</code> の依存・出版トピックは以下のとおりである:</p>
<div class="sourceCode" id="cb6"><pre class="sourceCode bash"><code class="sourceCode bash"><a class="sourceLine" id="cb6-1" data-line-number="1">[<span class="ex">sakurai@</span> ~]$ rosnode info /object_detector_3d</a>
<a class="sourceLine" id="cb6-2" data-line-number="2"><span class="ex">--------------------------------------------------------------------------------</span></a>
<a class="sourceLine" id="cb6-3" data-line-number="3"><span class="ex">Node</span> [/object_detector_3d]</a>
<a class="sourceLine" id="cb6-4" data-line-number="4"><span class="ex">Publications</span>:</a>
<a class="sourceLine" id="cb6-5" data-line-number="5"> <span class="ex">*</span> /object_detection_3d [object_detector_3d/Detection3DResult]</a>
<a class="sourceLine" id="cb6-6" data-line-number="6"> <span class="ex">*</span> /object_detection_3d/result_image [sensor_msgs/Image]</a>
<a class="sourceLine" id="cb6-7" data-line-number="7"> <span class="ex">*</span> /rosout [rosgraph_msgs/Log]</a>
<a class="sourceLine" id="cb6-8" data-line-number="8"></a>
<a class="sourceLine" id="cb6-9" data-line-number="9"><span class="ex">Subscriptions</span>:</a>
<a class="sourceLine" id="cb6-10" data-line-number="10"> <span class="ex">*</span> /camera/color/image_raw [sensor_msgs/Image]</a>
<a class="sourceLine" id="cb6-11" data-line-number="11"> <span class="ex">*</span> /camera/depth/color/points [sensor_msgs/PointCloud2]</a>
<a class="sourceLine" id="cb6-12" data-line-number="12"></a>
<a class="sourceLine" id="cb6-13" data-line-number="13"><span class="ex">Services</span>:</a>
<a class="sourceLine" id="cb6-14" data-line-number="14"> <span class="ex">*</span> /object_detector_3d/get_loggers</a>
<a class="sourceLine" id="cb6-15" data-line-number="15"> <span class="ex">*</span> /object_detector_3d/set_logger_level</a>
<a class="sourceLine" id="cb6-16" data-line-number="16"><span class="ex">...</span></a></code></pre></div>
<p>さらに依存トピックがこれらに加えて、</p>
<pre><code>sensor_msgs.CameraInfo
realsense2_camera.Extrinsics</code></pre>
<p>も必要となる。</p>
<h2 id="購読トピック">購読トピック</h2>
<p>各購読トピックの意味は次のとおりである:</p>
<ul>
<li><code>/camera/color/image_raw</code>: カラー画像。2D物体検出に使う。</li>
<li><code>/camera/depth/color/points</code> 3D点群。3次元位置を求めるために使う。そのため、上記の <code>/camera/color/image_raw</code> と時間的に同期が取れている必要がある。</li>
<li><code>sensor_msgs.CameraInfo</code>: <code>/camera/color/image_raw</code> のカメラの内部パラメタ。</li>
<li><code>realsense2_camera.Extrinsics</code>: 点群の座標系からカラーカメラ座標系へと変換するための外部パラメタ。なお、これはRealSenseの独自型であるが、アルゴリズム上必要となるのは回転・並進パラメタのみである。</li>
</ul>
<h2 id="出版トピック">出版トピック</h2>
<p>各出版トピックの意味は次のとおりである:</p>
<ul>
<li><p><code>/object_detection_3d</code>: 1対の画像と点群から得られる検出結果を格納する独自型であり、</p>
<pre><code>int32 num_detections
Detection3D[] detections</code></pre>
<p>となっている。 <code>detections</code> は複数の <code>Detection3D</code> を含む。 <code>Detection3D</code> は1つの検出を格納する独自型であり、</p>
<pre><code>int32 class_id
string class_name
float32 score
float32 y_min
float32 x_min
float32 y_max
float32 x_max
geometry_msgs/Point position</code></pre>
<p>となっている。 <code>class_id</code> 、 <code>class_name</code> はそれぞれ整数のクラス番号と文字列のクラス名であり、 <code>score</code> は確信度である。<code>y_min, x_min, y_max, x_max</code> は2D検出における画像座標系におけるbounding box (bbox) の左上と右下の頂点の座標である。最後に</p>
<p><code>position</code> が3次元位置である。</p></li>
<li><p><code>/object_detection_3d/result_image</code>: 検出に用いた画像に<code>Detection3D</code> の情報を書き込んだものになっており、前記の図の右上のような画像である。</p></li>
</ul>
<h1 id="実装の詳細">実装の詳細</h1>
<h2 id="入出力データ">入出力データ</h2>
<p>本ソフトウェアはカメラと点群により計測したシーン中に存在する複数の物体を検出することができる。</p>
<p>具体的には、入力として以下のデータを用いる:</p>
<ul>
<li>2Dカメラ画像</li>
<li>3D点群</li>
<li>カメラの内部パラメタ</li>
<li>点群座標系におけるカメラの配置を表す外部パラメタ</li>
</ul>
<p>出力として、各物体について以下の情報が得られる:</p>
<ul>
<li>点群座標系における物体の中心点の座標</li>
<li>物体の種類</li>
<li>検出の確信度</li>
</ul>
<h2 id="アルゴリズムの概要">アルゴリズムの概要</h2>
<p>以下の手順で物体の3D座標を求める:</p>
<ol type="1">
<li>カメラから取得した画像を入力として、物体検出器を用いて画像中の複数個の物体を検出する。</li>
<li>各検出に対応するbbox (bounding box) について、点群座標系における視錐台（frustum）を求める。</li>
<li>各検出に対応する視錐台について、その中に含まれる点群の部分集合を抽出する。</li>
<li>各部分点群について、中心点の座標を算出する。</li>
<li>2D検出と3D中心点座標を統合して、3D検出結果とする。</li>
</ol>
<h2 id="各アルゴリズムの説明">各アルゴリズムの説明</h2>
<h3 id="d物体検出">(1) 2D物体検出</h3>
<p>2D物体検出器とは、画像に含まれる（事前に定義された種類の）物体を検出するものをいう。検出とは、2D画像を入力すると、複数個の物体のそれぞれについて以下の情報を予測として出力することである:</p>
<ul>
<li>物体を囲む矩形（axis aligned bounding box, bbox）</li>
<li>物体の種類</li>
<li>予測の確信度</li>
</ul>
<p>具体的に用いる物体検出器は、 <a href="https://arxiv.org/abs/1512.02325">SSD300</a> を <a href="http://cocodataset.org/">MS COCO</a> データセットにより訓練したものであり、ConvNetによる80クラス物体検出器である。具体的な全てのクラスは<a href="https://github.com/chainer/chainercv/blob/64ee516fb8c6f750d6940de81900a62aaee9fff8/chainercv/datasets/coco/coco_utils.py#L89-L168">リンク先のリストを参照</a>されたい。</p>
<p>実装はpythonの深層学習フレームワークの <a href="https://github.com/chainer/chainer">chainer</a> に依存しており, 特に画像用途向けの拡張パッケージである <a href="https://github.com/chainer/chainercv">chainercv</a> を用いている。</p>
<h3 id="部分点群の抽出">(2, 3)部分点群の抽出</h3>
<p>ここでは前ステップで得たbbox情報に依存している。個別のbboxについて、点群の全ての点のうち、カメラ視点からみてbboxの内側に入る点のみを部分点群として抽出している。</p>
<h3 id="部分点群の中心点の算出">(4) 部分点群の中心点の算出</h3>
<p>部分点群は、対象物体とそれ以外の背景や遮蔽物に由来する点からなる。これらの点群を代表する1点に集約した点を、ここでは中心点と呼ぶ。</p>
<p>中心点の定義には様々なものが考えられるが、本ソフトウェアでは部分点群の重心を中心点として定義した。</p>
<p>ただし、この手法は物体と非物体を区別せずにデータとして用いているため、物体の形状やbboxのズレなどによっては物体そのものの重心から離れた座標が算出されてしまう可能性がある。そのため、例えば、非物体を除去したうえで残った点群の重心をとる、などのより良い中心点の定義が考えられる。</p>
<h3 id="d検出と3d中心点座標の統合">(5) 2D検出と3D中心点座標の統合</h3>
<p>単純なため省略する。</p>
<h1 id="ソフトウェアの保守拡張について">ソフトウェアの保守・拡張について</h1>
<h2 id="独自の2d物体検出器を利用する場合">独自の2D物体検出器を利用する場合</h2>
<p>同梱されている2D物体検出器は標準的な物体の種類に対応しているが、タスクに特化した検出器を別途作成した場合などに、それに取り替えることができる。特に、フレームワークに依存していないため、Chainer以外を利用することもできる。</p>
<p>そのためには、インタフェースクラスとして <code>object_detectors.BaseObjectDetector</code> で自前の検出器をラップし、 <code>predict</code> メソッドと <code>class_name_from_number</code> プロパティを実装したクラスを作成したうえで、<code>main_object_detector_3d.py#L167</code>の</p>
<div class="sourceCode" id="cb10"><pre class="sourceCode python"><code class="sourceCode python"><a class="sourceLine" id="cb10-1" data-line-number="1">    detector_2d <span class="op">=</span> COCOSSD300Detector()</a></code></pre></div>
<p>を自前のクラスと置き換えればよい。</p>
<h2 id="realsense以外のセンサーを利用する場合">RealSense以外のセンサーを利用する場合</h2>
<p>本実装では、時間同期のとれた画像と点群を取得する必要があるためRealSenseを用いている。また、外部パラメタのデータ形式として <code>realsense2_camera</code> パッケージの独自型を用いている。そのため、RealSense以外のセンサーを利用する場合には、<code>main_object_detector_3d.py#L86-87</code>の</p>
<div class="sourceCode" id="cb11"><pre class="sourceCode python"><code class="sourceCode python"><a class="sourceLine" id="cb11-1" data-line-number="1">    R <span class="op">=</span> np.array(extrinsics.rotation).reshape(<span class="dv">3</span>, <span class="dv">3</span>)</a>
<a class="sourceLine" id="cb11-2" data-line-number="2">    t <span class="op">=</span> np.array(extrinsics.translation)</a></code></pre></div>
<p>を修正する必要がある。回転・並進パラメタが取得できたあとの箇所については修正の必要はない。</p>
</body>
</html>
